namespace Microsoft.Robotics.Navigation.UnitTests
{
    using System;    
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Diagnostics.CodeAnalysis;
    using System.Drawing;
    using System.Linq;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Local Ev4 Suggestion drive tests
    /// </summary>
    [TestClass]
    public class LocalEv4SuggestionDriveTests
    {
        /// <summary>
        /// Tests if StateIsClearWhenJustCreated
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void StateIsClearWhenJustCreated()
        {
            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.5, 0.25);

            Assert.AreEqual(0, state.ConsecutiveFramesWithObstacles);
            Assert.AreEqual(0, state.LastTimeOASessionCreated);
            Assert.IsNull(state.LastSafeVelocity);
        }

        /// <summary>
        /// Tests if OATriggerTimeIsUpdatedWhenNewVelocityObjectIsCreated
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void OATriggerTimeIsUpdatedWhenNewVelocityObjectIsCreated()
        {
            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.5, 0.25);
            state.CurrentTimeStampInMS = Stopwatch.GetTimestamp();

            var vel = state.CreateNewSafeVelocity(1);

            Assert.AreEqual(0, state.ConsecutiveFramesWithObstacles);
            Assert.AreNotEqual(0, state.LastTimeOASessionCreated);
            Assert.IsNotNull(state.LastSafeVelocity);
        }

        /// <summary>
        /// Tests if CreateNewSafeVelocityRespectsDirection
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void CreateNewSafeVelocityRespectsDirection()
        {
            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.5, 0.25);

            var vel = state.CreateNewSafeVelocity(1);
            Assert.IsTrue(vel.ThetaInRadiansPerSecond > 0);

            vel = state.CreateNewSafeVelocity(-1);
            Assert.IsTrue(vel.ThetaInRadiansPerSecond < 0);
        }

        /// <summary>
        /// Tests if CreateNewSafeVelocityThrowsWhenAskedToGenerateEscapeRotationWithNoSuggestedDirection
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        [ExpectedException(typeof(InvalidOperationException))]
        public void CreateNewSafeVelocityThrowsWhenAskedToGenerateEscapeRotationWithNoSuggestedDirection()
        {
            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.5, 0.25);

            var vel = state.CreateNewSafeVelocity(0);            
        }

        /// <summary>
        /// Tests if GeneratedOAVelocitiesAreWithinSpecifiedLimits
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void GeneratedOAVelocitiesAreWithinSpecifiedLimits()
        {
            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.1, 5.5, 1.0);

            int iterations = 100;

            for (int i = 0; i < iterations; ++i)
            {
                var vel = state.CreateNewSafeVelocity(1);

                Assert.IsTrue(vel.ThetaInRadiansPerSecond >= 0.1);
                Assert.IsTrue(vel.ThetaInRadiansPerSecond <= 5.5);
            }

            for (int i = 0; i < iterations; ++i)
            {
                var vel = state.CreateNewSafeVelocity(-1);
                Assert.IsTrue(vel.ThetaInRadiansPerSecond <= -0.1);
                Assert.IsTrue(vel.ThetaInRadiansPerSecond >= -5.5);
            }
        }

        /// <summary>
        /// Tests if GeneratedOAVelocitiesAreSomewhatRandom
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void GeneratedOAVelocitiesAreSomewhatRandom()
        {
            double notSoSmallEpsilon = 0.1;
            double minSpeed = 0.3;
            double maxSpeed = 2;

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state =
                new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(minSpeed, maxSpeed, 0.5);

            double avg = 0;
            int iterations = 1000;

            for (int i = 0; i < iterations; ++i)
            {
                avg += state.CreateNewSafeVelocity(1).ThetaInRadiansPerSecond;
            }

            avg /= iterations;

            // Since we use a squared gaussian around min value, and a relatively large number of iterations, 
            // this upper should work virtually always. If this test ever fails - we need to tighten the gaussian
            double upperLimit = (minSpeed + maxSpeed) / 2;

            Assert.IsTrue(avg > minSpeed + notSoSmallEpsilon);
            Assert.IsTrue(avg < upperLimit - notSoSmallEpsilon);
        }

        /// <summary>
        /// Tests if SafeSpeedNotNullIfDepthFrameIsInvalid
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SafeSpeedNotNullIfDepthFrameIsInvalid()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000, 
                driveForwardSpeedAfterAvoiding: 0.5, 
                probabilityOfRespectingSuggestedDirection: 0.3);

            MobileBaseVelocityReference inputVel = new MobileBaseVelocityReference(0.3, 1.0, 0.3);

            var outputVel = sd.SafeSpeedFromSuggestedSpeed(null, inputVel);

            Assert.AreSame(outputVel, inputVel);
        }

        /// <summary>
        /// Tests if NoOAIsTriggeredOnFirstFrameWithObstacle
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void NoOAIsTriggeredOnFirstFrameWithObstacle()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.3);

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);

            MobileBaseVelocityReference inputVel = new MobileBaseVelocityReference();
            inputVel.XInMetersPerSecond = 1;
            inputVel.ThetaInRadiansPerSecond = 1;

            MobileBaseVelocityReference outputVel = sd.ProduceObstacleEscapeVelocity(state, inputVel, -1, new Random());
            Assert.AreSame(inputVel, outputVel);
        }

        /// <summary>
        /// Tests if EscapeVelocityDirectionRespectsSuggestedDirection
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void EscapeVelocityDirectionRespectsSuggestedDirection()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 1);

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);

            MobileBaseVelocityReference inputVel = new MobileBaseVelocityReference();
            inputVel.XInMetersPerSecond = 1;
            inputVel.ThetaInRadiansPerSecond = 1;

            // First frame with obstacle - no OA will be triggerred
            sd.ProduceObstacleEscapeVelocity(state, inputVel, -1, new Random());

            // Second frame with obstacle - OA triggerred, and even though we feed in negative direction of obstacle, 
            // the input should drive the direction
            MobileBaseVelocityReference outputVel = sd.ProduceObstacleEscapeVelocity(state, inputVel, -1, new Random());
            Assert.IsTrue(outputVel.ThetaInRadiansPerSecond > 0);
        }

        /// <summary>
        /// Tests if SameEscapeVelocityObjectIsUsedForASingleOASession
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SameEscapeVelocityObjectIsUsedForASingleOASession()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.3);

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);

            MobileBaseVelocityReference inputVel = new MobileBaseVelocityReference();
            inputVel.XInMetersPerSecond = 1;
            inputVel.ThetaInRadiansPerSecond = 1;

            // First frame with obstacle - no OA will be triggerred
            sd.ProduceObstacleEscapeVelocity(state, inputVel, -1, new Random());

            // Second frame with obstacle - OA triggerred. We want to see if the same object will be used for this session
            MobileBaseVelocityReference outputVel1 = sd.ProduceObstacleEscapeVelocity(state, inputVel, -1, new Random());

            // Pretend there are 100 frames with an obstacle in the view
            for (int i = 0; i < 100; ++i)
            {
                sd.ProduceObstacleEscapeVelocity(state, inputVel, -1, new Random());
            }

            // Change direction, still same object and direction
            MobileBaseVelocityReference outputVel2 = sd.ProduceObstacleEscapeVelocity(state, inputVel, 1, new Random());

            Assert.AreSame(outputVel1, outputVel2);
            Assert.AreNotSame(outputVel1, inputVel);
        }

        /// <summary>
        /// Tests if SuggestionDriveResetsStateWhenClearingAppears
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SuggestionDriveResetsStateWhenClearingAppears()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.3);

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);

            MobileBaseVelocityReference inputVel1 = new MobileBaseVelocityReference();
            inputVel1.XInMetersPerSecond = 1;
            inputVel1.ThetaInRadiansPerSecond = 1;

            // First frame with obstacle - no OA will be triggerred
            sd.ProduceObstacleEscapeVelocity(state, inputVel1, -1, new Random());

            // Second frame with obstacle - OA triggerred. Keep the object
            MobileBaseVelocityReference outputVel1 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, -1, new Random());

            // Third frame no obstacle, this should re-set the OA state
            state.CurrentTimeStampInMS = 1000; // disable overshoot
            state.LastTimeObstacleSeen = 0; // disable overshoot
            var outVel = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 0, new Random()); // 0 - is open space
            Assert.AreSame(outVel, inputVel1);

            // Fourth frame - obstacle again. No OA should trigger, since we wait for at least obstacle-present frames 2 in a row
            var outVel4 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, new Random());
            Assert.AreSame(inputVel1, outVel4);

            // Fifth frame - OA triggerred with new object
            var outVel5 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, new Random());
            Assert.AreNotSame(inputVel1, outVel5);            
        }

        /// <summary>
        /// Tests if SuggestionDriveReversesDirectionWhenCasterIsSuspectedToHaveBeenTangled
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SuggestionDriveReversesDirectionWhenCasterIsSuspectedToHaveBeenTangled()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.3);

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);
            state.CurrentTimeStampInMS = Stopwatch.GetTimestamp();

            MobileBaseVelocityReference inputVel1 = new MobileBaseVelocityReference();
            inputVel1.XInMetersPerSecond = 1;
            inputVel1.ThetaInRadiansPerSecond = 1;

            // First frame with obstacle - no OA will be triggerred
            sd.ProduceObstacleEscapeVelocity(state, inputVel1, -1, new Random());

            // Second frame with obstacle - OA triggerred. Keep the object
            var outVel1 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, new Random());
            Assert.IsTrue(outVel1.ThetaInRadiansPerSecond > 0);

            // Pretend its been a long time since OA first triggered
            state.LastTimeOASessionCreated = 0;

            // Third frame with obstacle - OA should reverse the state with new velocity object even though suggested direction
            // is same as before
            var outVel2 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, new Random());
            Assert.AreNotSame(outVel1, outVel2);
            Assert.IsTrue(outVel2.ThetaInRadiansPerSecond < 0);
        }

        /// <summary>
        /// Tests if SuggestionDriveUsesRandomizedDirectionSuggestionPicking
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SuggestionDriveUsesRandomizedDirectionSuggestionPicking()
        {
            // Try 1000 times to generate direction with 50/0 chance of obeying suggested direction and see if resulting chosen direction
            // for OA is close to half/half split
            int numberOfTimesWeRespectSuggestedDirection = 0;

            var rand = new Random();    //// we pass a global random var in, since we need it re-seed between runs, 
                                        //// and it does so at a fairly low frequency (seed from tick count internally)?

            for (int i = 0; i < 1000; ++i)
            {
                Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                    sonarThresholdDistanceInMM: 400,
                    depthThresholdInMM: 540,
                    desiredOverShootMillisecondsWhileTurning: 200,
                    maxMillisecondsDrivingForwardAfterTurning: 1000,
                    driveForwardSpeedAfterAvoiding: 0.5,
                    probabilityOfRespectingSuggestedDirection: 0.3);

                Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);

                MobileBaseVelocityReference inputVel1 = new MobileBaseVelocityReference();
                inputVel1.XInMetersPerSecond = 1;
                inputVel1.ThetaInRadiansPerSecond = -1;

                // First frame with obstacle - no OA will be triggerred
                sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, rand);

                // Seond frame should trigger OA with 0.7 preference towards dis-respecting suggested direction
                var outVel1 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, rand);

                numberOfTimesWeRespectSuggestedDirection += (Math.Sign(outVel1.ThetaInRadiansPerSecond) == Math.Sign(inputVel1.ThetaInRadiansPerSecond)) ? 1 : 0;
            }

            // While there is no 100% guarantee that out of 1000 random samples 400 will be below 0.5 and 600 will be above - there is a very good chance 
            // that the number will fall in that range, assuming the 0.5 chance of going in either direction
            Assert.IsTrue(numberOfTimesWeRespectSuggestedDirection > 200);
            Assert.IsTrue(numberOfTimesWeRespectSuggestedDirection < 400);
        }

        /// <summary>
        /// Tests if SuggestionDriveDrivesForwardByARandomizedAmount
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SuggestionDriveDrivesForwardByARandomizedAmount()
        {
            // Try 1000 times to generate direction with 50/0 chance of obeying suggested direction and see if resulting chosen direction
            // for OA is close to half/half split
            long totalMillisecondsForOverDrive = 0;
            int millisecondsToDriveForward = 10000;
            int numberOfTrials = 1000;

            var rand = new Random();    //// we pass a global random var in, since we need it re-seed between runs, 
            //// and it does so at a fairly low frequency (seed from tick count internally)?

            for (int i = 0; i < numberOfTrials; ++i)
            {
                Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                    sonarThresholdDistanceInMM: 400,
                    depthThresholdInMM: 540,
                    desiredOverShootMillisecondsWhileTurning: 200,
                    maxMillisecondsDrivingForwardAfterTurning: millisecondsToDriveForward,
                    driveForwardSpeedAfterAvoiding: 0.5,
                    probabilityOfRespectingSuggestedDirection: 0.3);

                Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);

                MobileBaseVelocityReference inputVel1 = new MobileBaseVelocityReference();
                inputVel1.XInMetersPerSecond = 1;
                inputVel1.ThetaInRadiansPerSecond = -1;

                // First frame with obstacle - no OA will be triggerred
                sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, rand);

                // Seond frame should trigger OA 
                var outVel1 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, rand);

                // For the third frame we simulate clearnace and see if random forward travel time is generated
                inputVel1.XInMetersPerSecond = 0; // no FW command from upstream means enter overturn state
                sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, rand);

                totalMillisecondsForOverDrive += state.RandomTimeInMSToDriveStraightAfterClearing;
            }

            totalMillisecondsForOverDrive /= numberOfTrials;

            Assert.IsTrue(totalMillisecondsForOverDrive > millisecondsToDriveForward / 2); // test if we are biased towards higher half of overdrive
            Assert.IsTrue(totalMillisecondsForOverDrive < millisecondsToDriveForward - millisecondsToDriveForward / 10); // test if we are not TOO biased towards higher half of overdrive
        }

        /// <summary>
        /// Tests if SuggestionDriveOvershootsAfterClearingAppears
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SuggestionDriveOvershootsAfterClearingAppears()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);
            state.CurrentTimeStampInMS = Stopwatch.GetTimestamp();

            MobileBaseVelocityReference inputVel1 = new MobileBaseVelocityReference();
            inputVel1.XInMetersPerSecond = 1;
            inputVel1.ThetaInRadiansPerSecond = 1;            

            // First frame with obstacle - no OA will be triggerred
            sd.ProduceObstacleEscapeVelocity(state, inputVel1, -1, new Random());

            // Second frame with obstacle - OA triggerred. Keep the object
            var outVel2 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 1, new Random());
            Assert.IsTrue(outVel2.ThetaInRadiansPerSecond > 0);

            // Third frame with no obstacle - still OA should overshoot
            var outVel3 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 0, new Random());
            Assert.AreSame(outVel2, outVel3);

            // Now pretend its been a long time since obstacles have been seen
            state.LastTimeObstacleSeen = 0;

            // Fourth frame, still no obstacle, but its been a while - overshoot should stop
            var outVel4 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 0, new Random());
            Assert.AreSame(inputVel1, outVel4);
        }

        /// <summary>
        /// Tests if SuggestionDriveOvershootsBySetAmountAfterClearingAppears
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SuggestionDriveOvershootsBySetAmountAfterClearingAppears()
        {
            int overshootDelay = 100;
            int overdriveDelay = 10000; 

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: overshootDelay,
                maxMillisecondsDrivingForwardAfterTurning: overdriveDelay,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);

            MobileBaseVelocityReference inputVel1 = new MobileBaseVelocityReference();
            inputVel1.XInMetersPerSecond = 1;
            inputVel1.ThetaInRadiansPerSecond = 1;

            long synthesizedTimestamp = 1000 * 1000 * 1000;

            // First frame with obstacle - no OA will be triggerred
            state.CurrentTimeStampInMS = synthesizedTimestamp;
            sd.ProduceObstacleEscapeVelocity(state, inputVel1, -1, new Random());            

            // Second frame with obstacle - OA triggerred. Keep the object
            state.CurrentTimeStampInMS = synthesizedTimestamp;
            var outVel2 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, -1, new Random());
            Assert.IsTrue(outVel2.ThetaInRadiansPerSecond != 0);
            Assert.AreNotSame(inputVel1, outVel2);

            // Third frame arriving just before overshoot expires, no obstacle, - still OA should be in control
            state.CurrentTimeStampInMS = synthesizedTimestamp + overshootDelay - 1;
            var outVel3 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 0, new Random());
            Assert.AreSame(outVel2, outVel3);

            // Fourth frame arriving just after overshoot expires, no obstacle, - OA should stop turning, and should start driving forward
            state.CurrentTimeStampInMS = synthesizedTimestamp + overshootDelay + 1;
            var outVel4 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 0, new Random());
            Assert.AreNotSame(outVel2, outVel4);
            Assert.AreEqual(0.5, outVel4.XInMetersPerSecond);  // Verify that we are driving forward
            Assert.AreEqual(0.0, outVel4.ThetaInRadiansPerSecond); // And not turning
            Assert.AreEqual(0.0, outVel4.YInMetersPerSecond);

            // Fifth frame arriving just after overdrive expires, no obstacle, - OA should stop driving forward
            state.CurrentTimeStampInMS = synthesizedTimestamp + overshootDelay + overdriveDelay;
            var outVel5 = sd.ProduceObstacleEscapeVelocity(state, inputVel1, 0, new Random());
            Assert.AreSame(inputVel1, outVel5); // OA fully disengaged and released control back to upstream component
        }

        /// <summary>
        /// Tests if ObstacleIsNotDetectedWhenFrameIsFullOfGoodReadings
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void ObstacleIsNotDetectedWhenFrameIsFullOfGoodReadings()
        {
            short tooCloseDepthInMM = 540;

            // Create a depth frame with valid readings that are farther than min distance
            Size size = new Size(320, 240);
            short[] frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: tooCloseDepthInMM,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            double direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);

            Assert.AreEqual(0, direction); // 0 == no obstacle
        }

        /// <summary>
        /// Tests if ObstacleIsDetectedWhenFrameContainsLotsOfNoReadingsColumns
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void ObstacleIsDetectedWhenFrameContainsLotsOfNoReadingsColumns()
        {
            short tooCloseDepthInMM = 540;

            // Create a depth frame with valid readings that are farther than min distance
            Size size = new Size(320, 240);
            short[] frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);

            for (int i = 0; i < size.Height; ++i)
            {
                int stride = i * size.Width;

                for (int j = size.Width / 2; j < size.Width / 2 + size.Width / 5; ++j)
                {
                    frame[stride + j] = 0; // no readings
                }
            }

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: tooCloseDepthInMM,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            double direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);

            Assert.AreNotEqual(0, direction); // 0 == no obstacle
        }

        /// <summary>
        /// Tests if ObstacleNotDetectedWhenFrameContainsTooFewOfNoReadingsColumns
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void ObstacleNotDetectedWhenFrameContainsTooFewOfNoReadingsColumns()
        {
            short tooCloseDepthInMM = 540;

            // Create a depth frame with valid readings that are farther than min distance
            Size size = new Size(320, 240);
            short[] frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);

            for (int i = 0; i < size.Height; ++i)
            {
                int stride = i * size.Width;

                // Thin slice of 
                int endScan = size.Width / 2 + size.Width / Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution - 1;

                for (int j = size.Width / 2; j < endScan; ++j)
                {
                    frame[stride + j] = 0; // no readings
                }
            }

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: tooCloseDepthInMM,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            double direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);

            Assert.AreEqual(0, direction); // 0 == no obstacle
        }

        /// <summary>
        /// Tests if ObstacleIsDetectedWhenFrameContainsEvenSingleTooCloseReading
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void ObstacleIsDetectedWhenFrameContainsEvenSingleTooCloseReading()
        {
            short tooCloseDepthInMM = 540;

            // Create a depth frame with valid readings that are farther than min distance
            Size size = new Size(320, 240);
            short[] frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);

            frame[size.Width / 2] = (short)(tooCloseDepthInMM - 1);

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: tooCloseDepthInMM,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            double direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);

            Assert.AreNotEqual(0, direction); // 0 == no obstacle
        }

        /// <summary>
        /// Tests if ObstacleIsNotDetectedWhenObstacleIsInInDecimatedArea
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void ObstacleIsNotDetectedWhenObstacleIsInInDecimatedArea()
        {
            short tooCloseDepthInMM = 540;

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: tooCloseDepthInMM,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            // Create a depth frame with valid readings that are farther than min distance
            Size size = new Size(320, 240);

            short[] frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);

            // Set an obstacle off one pixel from decimated area. This way we verify that we dont scan the whole image
            frame[size.Width / 2 + Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution + 1] = (short)(tooCloseDepthInMM - 1);

            double direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);
            Assert.AreEqual(0, direction); // 0 == no obstacle
        }

        /// <summary>
        /// Tests if ObstacleDirectionIsCorrectWhenFrameContainsTooCloseReadings
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void ObstacleDirectionIsCorrectWhenFrameContainsTooCloseReadings()
        {
            short tooCloseDepthInMM = 540;

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: tooCloseDepthInMM,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            // Create a depth frame with valid readings that are farther than min distance
            Size size = new Size(320, 240);
            short[] frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);

            for (int i = 0; i < size.Height; ++i)
            {
                int stride = i * size.Width;

                // Thin slice of 
                int startScan = size.Width / 2 + size.Width / Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution * 2;
                int endScan = startScan + size.Width / Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution * 2;

                for (int j = startScan; j < endScan; ++j)
                {
                    frame[stride + j] = 0; // no readings
                }
            }

            double direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);

            Assert.AreEqual(-1, direction); 
            
            // Test the other side
            frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);

            for (int i = 0; i < size.Height; ++i)
            {
                int stride = i * size.Width;

                // Thin slice of 
                int startScan = size.Width / 2 - size.Width / Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution * 3;
                int endScan = startScan + size.Width / Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution * 2;

                for (int j = startScan; j < endScan; ++j)
                {
                    frame[stride + j] = 0; // no readings
                }
            }

            direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);
            Assert.AreEqual(1, direction); 
        }

        /// <summary>
        /// Tests if ObstacleDirectionIsCorrectWhenFrameContainsNoReadings
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void ObstacleDirectionIsCorrectWhenFrameContainsNoReadings()
        {
            short tooCloseDepthInMM = 540;

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: tooCloseDepthInMM,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            // Create a depth frame with valid readings that are farther than min distance
            Size size = new Size(320, 240);

            short[] frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);

            // Simulate obstacle just one scan line to the right of center
            frame[size.Width / 2 + Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution] = (short)(tooCloseDepthInMM - 1);
            double direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);
            Assert.AreEqual(-1, direction);

            // Simulate obstacle just one scan line to the left of center
            frame = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);
            frame[size.Width / 2 - Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution] = (short)(tooCloseDepthInMM - 1);
            direction = sd.GetNearestObstacleDirectionFromDepthFrame(frame, size);
            Assert.AreEqual(1, direction);
        }

        /// <summary>
        /// Tests if ObstacleDirectionIsCorrectAfterSensorVotesAreFused
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void ObstacleDirectionIsCorrectAfterSensorVotesAreFused()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            Assert.AreEqual(0, sd.FuseSensorVotes(0, 0));

            Assert.AreEqual(1, sd.FuseSensorVotes(0, 1));

            Assert.AreEqual(1, sd.FuseSensorVotes(1, 0));

            Assert.AreEqual(1, sd.FuseSensorVotes(-1, 1));

            Assert.AreEqual(-1, sd.FuseSensorVotes(1, -1));
        }

        /// <summary>
        /// Tests if ObstacleDirectionIsCorrectWhenSonarDoesNotFire
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void NoObstacleWhenSonarObjectIsInvalid()
        {
            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            Assert.AreEqual(0, sd.GetNearestObstacleDirectionFromSonar(null));

            List<SonarState> sonarState = this.SythesizeEv4SonarState(100, 100);
            sonarState.RemoveAt(0); // remove one of sonars - this should never happen

            Assert.AreEqual(0, sd.GetNearestObstacleDirectionFromSonar(sonarState));
        }

        /// <summary>
        /// Tests if SonarVotesProperDirectionWhenVariousInputsAreFedIn
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void SonarVotesProperDirectionWhenVariousInputsAreFedIn()
        {
            int tooCloseSonarInMM = 400;

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: tooCloseSonarInMM,
                depthThresholdInMM: 540,
                desiredOverShootMillisecondsWhileTurning: 200,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.5);

            int justABitTooClose = tooCloseSonarInMM - 1;
            int evenCloser = justABitTooClose - 1;
            int slightlyOutsideThreshold = tooCloseSonarInMM + 1;

            Assert.AreEqual(
                0, 
                sd.GetNearestObstacleDirectionFromSonar(
                    this.SythesizeEv4SonarState(0, 0)));

            Assert.AreEqual(
                0, 
                sd.GetNearestObstacleDirectionFromSonar(
                    this.SythesizeEv4SonarState(slightlyOutsideThreshold, slightlyOutsideThreshold)));

            // When both sonars trigger - we choose a direction (does not matter which way)
            Assert.AreNotEqual(
                0, 
                sd.GetNearestObstacleDirectionFromSonar(
                    this.SythesizeEv4SonarState(justABitTooClose, justABitTooClose)));

            // Left direction test
            Assert.AreEqual(
                -1, 
                sd.GetNearestObstacleDirectionFromSonar(
                    this.SythesizeEv4SonarState(justABitTooClose, slightlyOutsideThreshold)));
            
            Assert.AreEqual(
                -1, 
                sd.GetNearestObstacleDirectionFromSonar(
                    this.SythesizeEv4SonarState(evenCloser, justABitTooClose)));

            // Right direction test
            Assert.AreEqual(
                1, 
                sd.GetNearestObstacleDirectionFromSonar(
                    this.SythesizeEv4SonarState(slightlyOutsideThreshold, justABitTooClose)));

            Assert.AreEqual(
                1, 
                sd.GetNearestObstacleDirectionFromSonar(
                    this.SythesizeEv4SonarState(justABitTooClose, evenCloser)));     
        }

        /// <summary>
        /// Tests if FinalOASpeedFromBothDepthAndSonarSensorsIsCorrect
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Local Ev4 Suggestion Drive")]
        [Priority(0)]
        public void FinalOASpeedFromBothDepthAndSonarSensorsIsCorrect()
        {
            short tooCloseDepthInMM = 540;
            short tooCloseSonarInMM = 400;

            Size size = new Size(320, 240);

            Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState state = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(0.3, 1.0, 0.3);
            state.CurrentTimeStampInMS = Stopwatch.GetTimestamp();
            state.LastDepthImage = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);
            state.LastDepthImageSize = size;
            state.LastSonarState = this.SythesizeEv4SonarState(tooCloseSonarInMM + 1, tooCloseSonarInMM + 1);

            Ev4LocalSuggestionDrive sd = new Ev4LocalSuggestionDrive(
                sonarThresholdDistanceInMM: tooCloseSonarInMM,
                depthThresholdInMM: tooCloseDepthInMM,
                desiredOverShootMillisecondsWhileTurning: 0, //// Disable overshoot for this test
                maxMillisecondsDrivingForwardAfterTurning: 0, //// Disable overdrive for this test
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 1); //// respect the suggested direction for this test

            MobileBaseVelocityReference inputVel = new MobileBaseVelocityReference();
            inputVel.XInMetersPerSecond = 1;
            inputVel.ThetaInRadiansPerSecond = 1;

            // No obstacles on any sensor
            var outVel1 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);
            Assert.AreSame(inputVel, outVel1);

            // Sonar obstacle. First frame - no OA should trigger still
            state.LastSonarState = this.SythesizeEv4SonarState(tooCloseSonarInMM - 1, tooCloseSonarInMM + 1);
            var outVel2 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);            
            Assert.AreSame(inputVel, outVel2);

            // Sonar obstacle. Second frame - OA should trigger
            state.LastSonarState = this.SythesizeEv4SonarState(tooCloseSonarInMM - 1, tooCloseSonarInMM + 1);
            var outVel3 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);            
            Assert.AreNotSame(inputVel, outVel3);
            Assert.AreEqual(Math.Sign(inputVel.ThetaInRadiansPerSecond), Math.Sign(outVel3.ThetaInRadiansPerSecond));

            // Clearing - OA should stop (since we disabled overshoot and overdrive)
            state.LastSonarState = this.SythesizeEv4SonarState(tooCloseSonarInMM + 1, tooCloseSonarInMM + 1);
            var outVel4 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);            
            Assert.AreSame(inputVel, outVel4);

            // Kinect obstacle, but no sonar. First frame should not trigger OA
            state.LastDepthImage[size.Width / 2 + Ev4LocalSuggestionDrive.DepthFrameSkipRateAt640x480Resolution] = (short)(tooCloseDepthInMM - 1);
            var outVel5 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);
            Assert.AreSame(inputVel, outVel5);

            // Kinect obstacle still set (see above test), but no sonar. Second frame should trigger OA            
            var outVel6 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);
            Assert.AreNotSame(inputVel, outVel6);
            Assert.AreEqual(Math.Sign(inputVel.ThetaInRadiansPerSecond), Math.Sign(outVel6.ThetaInRadiansPerSecond));

            // Kinect obstacle still present + now Sonar obstacle too. Must keep origianal OA object
            state.LastSonarState = this.SythesizeEv4SonarState(tooCloseSonarInMM - 1, tooCloseSonarInMM + 1);
            var outVel7 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);
            Assert.AreSame(outVel6, outVel7);

            // Clear obstacles. No OA
            state.LastDepthImage = this.SythesizeDepthFrame(size, tooCloseDepthInMM + 1);
            state.LastSonarState = this.SythesizeEv4SonarState(tooCloseSonarInMM + 1, tooCloseSonarInMM + 1);
            var outVel8 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);
            Assert.AreSame(inputVel, outVel8);

            // New obstacle on both sensors. First frame - No OA
            state.LastDepthImage[size.Width / 2 + 1] = (short)(tooCloseDepthInMM - 1);
            state.LastSonarState = this.SythesizeEv4SonarState(tooCloseSonarInMM - 1, tooCloseSonarInMM + 1);
            var outVel9 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);
            Assert.AreSame(inputVel, outVel9);

            // New obstacle on both sensors. Second frame. OA Triggerrs - Sonar wins
            var outVel10 = sd.SafeSpeedFromSuggestedSpeed(state, inputVel);
            Assert.AreNotSame(inputVel, outVel10);
            Assert.AreEqual(Math.Sign(inputVel.ThetaInRadiansPerSecond), Math.Sign(outVel10.ThetaInRadiansPerSecond));
        }

        /// <summary>
        /// Generates a sonar state object
        /// </summary>
        /// <param name="leftDistanceInMM">Left distance</param>
        /// <param name="rightDistanceInMM">Right distance</param>
        /// <returns>Depth frame</returns>
        private List<SonarState> SythesizeEv4SonarState(int leftDistanceInMM, int rightDistanceInMM)
        {
            List<SonarState> sonarState = new List<SonarState>();

            SonarState left = new SonarState();
            left.DistanceInMeters = (double)leftDistanceInMM / 1000;
            sonarState.Add(left);

            SonarState right = new SonarState();
            right.DistanceInMeters = (double)rightDistanceInMM / 1000;
            sonarState.Add(right);

            return sonarState;
        }

        /// <summary>
        /// Generates a bogus depth frame
        /// </summary>
        /// <param name="size">Size of the frame</param>
        /// <param name="value">Fill with this</param>
        /// <returns>Depth frame</returns>
        private short[] SythesizeDepthFrame(Size size, int value) 
        {
            return Enumerable.Repeat((short)value, size.Height * size.Width).ToArray();
        }
    }  
}
